#include "safetool.h"

extern CAN_HandleTypeDef hcan;

void OLED_ErrorLog(int x, int y)
{

  uint8_t i, n;
	static S_e _error;
	
	if (_error != error){
		for (i = y; i < 8; i++) {
			OLED_WR_Byte(0xb0 + i, OLED_CMD);
			OLED_WR_Byte(0x00, OLED_CMD);
			OLED_WR_Byte(0x10, OLED_CMD);
			for (n = 0; n < 128; n++)
				OLED_WR_Byte(0, OLED_DATA);
		}
}
	
	_error = error;

    switch (error)
    {
    case E_NONE:
        OLED_Print(x, y, "Run Normal");
        break;
    case E_DISLR:
        OLED_Print(x, y, "Barbell bar not in middle");
        break;
    case E_EXHAUSTED:
        OLED_Print(x, y, "Exhausted");
        break;
    default:
        OLED_Print(x, y, "Run Normal");
    }
}


int HeightCheck(int Height ,int LRHeight){
	 static int cnt = 0;
	
	 if (abs(Height - LRHeight) > 30)
    {
      cnt ++;
    }
		
		if(cnt > 10)
		{
			cnt = 0;
			FLAG_EMERGENCY_BRAKING = 1;
      CAN_LoadDataFrame(MOTOR_ID, CMD_SAFE, 0x0001);
      HAL_CAN_Transmit_IT(&hcan);
      return true;
		}
    return false;
}


void SyncHeight(int Height ,int LRHeight){

}

void SovleCanErr()
{
	static int cnt_CanErr;
	if(FLAG_CAN != HAL_OK) 
	{
		cnt_CanErr ++;
		FLAG_CAN = HAL_CAN_Receive_IT(&hcan, CAN_FIFO0);
		if(cnt_CanErr > 10)
		{
			cnt_CanErr = 0;
			FLAG_EMERGENCY_BRAKING = 1;
			CAN_LoadDataFrame(MOTOR_ID, CMD_SAFE, 0x0001);
			HAL_CAN_Transmit_IT(&hcan);
		}
	}
}

